
#ifndef ISOFTBODYCOLLISIONSHAPE_H
#define ISOFTBODYCOLLISIONSHAPE_H

#include <string.h>
#include <cmath>

#include <BSoftBody>
#include "IBroadphase.h"
#include "ITriangleCallback.h"
#include "member_BSoftBody.h"

namespace BWE
{
	class ISoftBodyCollisionShape : public BShape
	{
	public:
		ISoftBodyCollisionShape(BSoftBody* body)
		{
			m_body = body;
		}

		virtual void copy(const BShape* other)
		{

		}
		virtual BShape* clone() const
		{
			return new ISoftBodyCollisionShape(*this);
		}

		void processTriangles(ITriangleCallback* callback, const BSpace& space) const
		{

		}

		virtual BSpace space(const BMatrix& matrix) const
		{
			const BVector mins = m_body->space().min();
			const BVector maxs = m_body->space().max();
			const BVector crns[] = {
				matrix.mult3(mins.x(), mins.y(), mins.z()),
				matrix.mult3(maxs.x(), mins.y(), mins.z()),
				matrix.mult3(maxs.x(), maxs.y(), mins.z()),
				matrix.mult3(mins.x(), maxs.y(), mins.z()),
				matrix.mult3(mins.x(), mins.y(), maxs.z()),
				matrix.mult3(maxs.x(), mins.y(), maxs.z()),
				matrix.mult3(maxs.x(), maxs.y(), maxs.z()),
				matrix.mult3(mins.x(), maxs.y(), maxs.z())
			};
			BVector aabbMin = crns[0];
			BVector aabbMax = crns[0];
			for (int i = 1; i < 8; ++i)
			{
				SetMin(aabbMin, crns[i]);
				SetMax(aabbMax, crns[i]);
			}
			return BSpace(aabbMin, aabbMax);
		}

	protected:
		virtual void refresh(BBox& box)
		{

		}
		virtual void render(BRender& render)
		{

		}
		virtual BVector vertex(const BVector& direct) const
		{
			return BVector();
		}
		virtual BVector inertia(BReal mass) const
		{
			return BVector();
		}

	public:
		BSoftBody* m_body;
	};

	class SoftClusterCollisionShape : public BShape
	{
	public:
		SoftClusterCollisionShape(const Cluster* cluster)
		{
			m_cluster = cluster;
			setMargin(0);
		}

		virtual void copy(const BShape* other)
		{

		}
		virtual BShape* clone() const
		{
			return new SoftClusterCollisionShape(*this);
		}

	protected:
		virtual void refresh(BBox& box)
		{

		}
		virtual void render(BRender& render)
		{

		}
		virtual BVector vertex(const BVector& direct) const
		{
			Vertex* const* nodes = &m_cluster->vertexs[0];
			BReal d = direct.dot(nodes[0]->position);
			int j = 0;
			for (int i = 1; i < m_cluster->vertexs.size(); ++i)
			{
				const BReal k = direct.dot(nodes[i]->position);
				if (k > d)
				{
					d = k;
					j = i;
				}
			}
			BVector vertex = nodes[j]->position;
			BReal margin = this->margin();
			if (margin != 0)
			{
				BVector dir = direct;
				if (direct.length2() < REAL_EPSILON)
					dir.set(-1);
				dir.normalize();
				vertex += dir * margin;
			}
			return vertex;
		}
		virtual BVector inertia(BReal mass) const
		{
			return BVector();
		}

		virtual BSpace space(const BMatrix& matrix) const
		{
			return BSpace();
		}

	public:
		const Cluster* m_cluster;

	};

	template <typename T>
	inline bool CompLess(const T& a, const T& b)
	{
		return (a < b);
	}

	template <typename T>
	inline bool CompGreater(const T& a, const T& b)
	{
		return (a > b);
	}

	inline BReal Clamp(BReal x, BReal l, BReal h)
	{
		return (x < l ? l : x > h ? h : x);
	}
	inline BVector Clamp(const BVector& v, BReal maxlength)
	{
		const BReal sql = v.length2();
		if (sql > (maxlength * maxlength))
			return ((v * maxlength) / sqrt(sql));
		else
			return (v);
	}

	inline BReal ClusterMetric(const BVector& x, const BVector& y)
	{
		const BVector d = x - y;
		return (fabs(d[0]) + fabs(d[1]) + fabs(d[2]));
	}

	inline BMatrix Cross(const BVector& v)
	{
		BMatrix m;
		m.setColumn(0, 0, -v.z(), +v.y());
		m.setColumn(1, +v.z(), 0, -v.x());
		m.setColumn(2, -v.y(), +v.x(), 0);
		return (m);
	}

	inline BMatrix Diagonal(BReal x)
	{
		BMatrix m;
		m.setColumn(0, x, 0, 0);
		m.setColumn(1, 0, x, 0);
		m.setColumn(2, 0, 0, x);
		return (m);
	}
	inline BMatrix Diagonal(const BVector& v)
	{
		BMatrix m;
		m.setColumn(0, v.x(), 0, 0);
		m.setColumn(1, 0, v.y(), 0);
		m.setColumn(2, 0, 0, v.z());
		return (m);
	}

	inline BReal Dot(const BReal* a, const BReal* b, int ndof)
	{
		BReal result = 0;
		for (int i = 0; i < ndof; ++i)
			result += a[i] * b[i];
		return result;
	}

	inline BMatrix OuterProduct(const BReal* v1, const BReal* v2, const BReal* v3, const BReal* u1, const BReal* u2, const BReal* u3, int ndof)
	{
		BMatrix m;
		BReal a11 = Dot(v1, u1, ndof);
		BReal a12 = Dot(v1, u2, ndof);
		BReal a13 = Dot(v1, u3, ndof);

		BReal a21 = Dot(v2, u1, ndof);
		BReal a22 = Dot(v2, u2, ndof);
		BReal a23 = Dot(v2, u3, ndof);

		BReal a31 = Dot(v3, u1, ndof);
		BReal a32 = Dot(v3, u2, ndof);
		BReal a33 = Dot(v3, u3, ndof);
		m.setColumn(0, a11, a12, a13);
		m.setColumn(1, a21, a22, a23);
		m.setColumn(2, a31, a32, a33);
		return (m);
	}
	inline BMatrix OuterProduct(const BVector& v1, const BVector& v2)
	{
		BMatrix m;
		BReal a11 = v1[0] * v2[0];
		BReal a12 = v1[0] * v2[1];
		BReal a13 = v1[0] * v2[2];

		BReal a21 = v1[1] * v2[0];
		BReal a22 = v1[1] * v2[1];
		BReal a23 = v1[1] * v2[2];

		BReal a31 = v1[2] * v2[0];
		BReal a32 = v1[2] * v2[1];
		BReal a33 = v1[2] * v2[2];
		m.setColumn(0, a11, a12, a13);
		m.setColumn(1, a21, a22, a23);
		m.setColumn(2, a31, a32, a33);
		return (m);
	}

	inline BMatrix Add(const BMatrix& a, const BMatrix& b)
	{
		return a + b;
	}
	inline BMatrix Sub(const BMatrix& a, const BMatrix& b)
	{
		return a - b;
	}
	inline BMatrix Mul(const BMatrix& a, BReal b)
	{
		return a * b;
	}

	inline BMatrix MassMatrix(BReal im, const BVector& invInertia, const BVector& r)
	{
		const BMatrix cr = Cross(r);
		return (Sub(Diagonal(im), cr * invInertia * cr));
	}
	inline BMatrix ImpulseMatrix(BReal dtime, BReal imassA, BReal imassB, const BVector& invInertiaB, const BVector& r)
	{
		return (Diagonal(1 / dtime) * Add(Diagonal(imassA), MassMatrix(imassB, invInertiaB, r)).inverse());
	}
	inline BMatrix ImpulseMatrix(BReal dt, const BMatrix& effective_mass_inv, BReal imb, const BVector& invInertiaB, const BVector& r)
	{
		return (Diagonal(1 / dt) * Add(effective_mass_inv, MassMatrix(imb, invInertiaB, r)).inverse());
	}
	inline BMatrix ImpulseMatrix(BReal ima, const BVector& invInertiaA, const BVector& ra, BReal imb, const BVector& invInertiaB, const BVector& rb)
	{
		return (Add(MassMatrix(ima, invInertiaA, ra), MassMatrix(imb, invInertiaB, rb)).inverse());
	}

	inline BMatrix AngularImpulseMatrix(const BMatrix& iia, const BMatrix& iib)
	{
		return (Add(iia, iib).inverse());
	}

	inline BVector ProjectOnAxis(const BVector& v, const BVector& a)
	{
		return (a * v.dot(a));
	}
	inline BVector ProjectOnPlane(const BVector& v, const BVector& a)
	{
		return (v - ProjectOnAxis(v, a));
	}

	inline void ProjectOrigin(const BVector& a, const BVector& b, BVector& prj, BReal& sqd)
	{
		const BVector d = b - a;
		const BReal m2 = d.length2();
		if (m2 > REAL_EPSILON)
		{
			const BReal t = Clamp(-a.dot(d) / m2, 0, 1);
			const BVector p = a + d * t;
			const BReal l2 = p.length2();
			if (l2 < sqd)
			{
				prj = p;
				sqd = l2;
			}
		}
	}
	inline void ProjectOrigin(const BVector& a, const BVector& b, const BVector& c, BVector& prj, BReal& sqd)
	{
		const BVector& q = (b - a).cross(c - a);
		const BReal m2 = q.length2();
		if (m2 > REAL_EPSILON)
		{
			const BVector n = q / sqrt(m2);
			const BReal k = a.dot(n);
			const BReal k2 = k * k;
			if (k2 < sqd)
			{
				const BVector p = n * k;
				if ((a - p).cross(b - p).dot(q) > 0 &&
					(b - p).cross(c - p).dot(q) > 0 &&
					(c - p).cross(a - p).dot(q) > 0)
				{
					prj = p;
					sqd = k2;
				}
				else
				{
					ProjectOrigin(a, b, prj, sqd);
					ProjectOrigin(b, c, prj, sqd);
					ProjectOrigin(c, a, prj, sqd);
				}
			}
		}
	}

	template <typename T>
	inline T BaryEval(const T& a, const T& b, const T& c, const BVector& coord)
	{
		return (a * coord.x() + b * coord.y() + c * coord.z());
	}

	inline BVector BaryCoord(const BVector& a, const BVector& b, const BVector& c, const BVector& p)
	{
		const BReal w0 = (a - p).cross(b - p).length();
		const BReal w1 = (b - p).cross(c - p).length();
		const BReal w2 = (c - p).cross(a - p).length();
		const BReal isum = 1 / (w0 + w1 + w2);
		return (BVector(w1 * isum, w2 * isum, w0 * isum));
	}

	inline BVector NormalizeAny(const BVector& v)
	{
		const BReal l = v.length();
		if (l > REAL_EPSILON)
			return (v / l);
		else
			return BVector();
	}

	inline BReal AreaOf(const BVector& x0, const BVector& x1, const BVector& x2)
	{
		const BVector a = x1 - x0;
		const BVector b = x2 - x0;
		const BVector c = a.cross(b);
		return c.length();
	}

	inline BSpace SpaceOf(const Face& face, BReal margin)
	{
		const BVector points[] = { face.vertex0->position, face.vertex1->position, face.vertex2->position };
		BSpace space(points[0]);
		space.expand(points[1]);
		space.expand(points[2]);
		space.min() -= margin;
		space.max() += margin;
		return space;
	}

	inline void ApplyClampedForce(Vertex& n, const BVector& f, BReal dt)
	{
		const BReal dtim = dt * n.imass;
		if ((f * dtim).length2() > n.velocity.length2())
		{
			n.force -= ProjectOnAxis(n.velocity, f.normal()) / dtim;
		}
		else
		{
			n.force += f;
		}
	}

	inline int PolarDecompose(const BMatrix& m, BMatrix& q, BMatrix& s)
	{
		class PolarDecomposition
		{
		public:
			PolarDecomposition()
			{

			}
			PolarDecomposition(BReal tolerance, BUInt maxIterations)
				: m_tolerance(tolerance), m_maxIterations(maxIterations)
			{
			}

			BReal abs_column_sum(const BMatrix& a, int i) const
			{
				return fabs(a[0][i]) + fabs(a[1][i]) + fabs(a[2][i]);
			}
			BReal abs_row_sum(const BMatrix& a, int i) const
			{
				return fabs(a[i][0]) + fabs(a[i][1]) + fabs(a[i][2]);
			}
			BReal p1_norm(const BMatrix& a) const
			{
				const BReal sum0 = abs_column_sum(a, 0);
				const BReal sum1 = abs_column_sum(a, 1);
				const BReal sum2 = abs_column_sum(a, 2);
				return bMax(bMax(sum0, sum1), sum2);
			}
			BReal pinf_norm(const BMatrix& a) const
			{
				const BReal sum0 = abs_row_sum(a, 0);
				const BReal sum1 = abs_row_sum(a, 1);
				const BReal sum2 = abs_row_sum(a, 2);
				return bMax(bMax(sum0, sum1), sum2);
			}

			BUInt decompose(const BMatrix& a, BMatrix& u, BMatrix& h) const
			{
				u = a;
				h = a.inverse();

				for (BUInt i = 0; i < m_maxIterations; ++i)
				{
					const BReal h_1 = p1_norm(h);
					const BReal h_inf = pinf_norm(h);
					const BReal u_1 = p1_norm(u);
					const BReal u_inf = pinf_norm(u);

					const BReal h_norm = h_1 * h_inf;
					const BReal u_norm = u_1 * u_inf;

					// The matrix is effectively singular so we cannot invert it
					if (bZero(h_norm) || bZero(u_norm))
						break;

					const BReal gamma = pow(h_norm / u_norm, 0.25f);
					const BReal inv_gamma = BReal(1.0) / gamma;

					// Determine the delta to 'u'
					const BMatrix delta = (u * (gamma - BReal(2.0)) + h.reverse() * inv_gamma) * BReal(0.5);

					// Update the matrices
					u += delta;
					h = u.inverse();

					// Check for convergence
					if (p1_norm(delta) <= m_tolerance * u_1)
					{
						h = u.reverse() * a;
						h = (h + h.reverse()) * 0.5;
						return i;
					}
				}

				// The algorithm has failed to converge to the specified tolerance, but we
				// want to make sure that the matrices returned are in the right form.
				h = u.reverse() * a;
				h = (h + h.reverse()) * 0.5;

				return m_maxIterations;
			}

			BUInt maxIterations() const
			{
				return m_maxIterations;
			}

		private:
			BReal m_tolerance;
			BUInt m_maxIterations;
		};
		static const PolarDecomposition polar;
		return polar.decompose(m, q, s);
	}
}
#endif
